home *** CD-ROM | disk | FTP | other *** search
/ Linux Cubed Series 8: LINUX Games / Linux Cubed Series 8 - LINUX Games.iso / games / video / fly8111-.000 / fly8111- / fly8 / sixdof.c < prev    next >
C/C++ Source or Header  |  1979-12-31  |  8KB  |  316 lines
  1. /* --------------------------------- sixdof.c ------------------------------- */
  2.  
  3. /* This is part of the flight simulator 'fly8'.
  4.  * Author: Eyal Lebedinsky (eyal@ise.canberra.edu.au).
  5. */
  6.  
  7. /* Resolve the plane's motion. The main program uses a different axes system
  8.  * than is common in aerodynamics. Here is the relationship:
  9.  * standard    fly8    meaning
  10.  * x         y    forward
  11.  * y         x    right
  12.  * z        -z    down
  13.  * p         dy    roll (clockwise)
  14.  * q         dx    pitch (up)
  15.  * r        -dz    yaw (right)
  16. */
  17.  
  18. #include "plane.h"
  19.  
  20. #define FUNDAMENTAL    (st.debug & DF_GPZ)
  21.  
  22. extern void FAR
  23. SixDOF (OBJECT *p, VECT F, VECT MM, MAT I)
  24. {
  25.     int    t, tt, onground;
  26.     AVECT    dae;
  27.  
  28.     onground = EX->flags & PF_ONGROUND;
  29.  
  30. /* This is the 'simple' way of doing the kinematics.
  31. */
  32. if (!FUNDAMENTAL) {
  33.     t = (int)(FONE/(C_PI/2*VONE));
  34.  
  35.     EX->a[Y] = F[Y] + muldiv (p->da[X], EX->v[Z], t)
  36.                 - muldiv (p->da[Z], EX->v[X], t);
  37.     EX->a[X] = F[X] - muldiv (p->da[Y], EX->v[Z], t)
  38.                 + muldiv (p->da[Z], EX->v[Y], t);
  39.     EX->a[Z] = F[Z] + muldiv (p->da[Y], EX->v[X], t)
  40.                 - muldiv (p->da[X], EX->v[Y], t);
  41.     EX->Gforce = F[Z];
  42.  
  43.     p->dda[X] = muldiv (MM[X], FONE/10, I[X][X]);
  44.     if (p->dda[X] > FONE/VONE)
  45.         p->dda[X] = FONE;
  46.     else if (p->dda[X] < -FONE/VONE)
  47.         p->dda[X] = -FONE;
  48.     else
  49.         p->dda[X] = muldiv (MM[X], FONE/10*VONE, I[X][X]);
  50.  
  51.     p->dda[Y] = muldiv (MM[Y], FONE/10, I[Y][Y]);
  52.     if (p->dda[Y] > FONE/VONE)
  53.         p->dda[Y] = FONE;
  54.     else if (p->dda[Y] < -FONE/VONE)
  55.         p->dda[Y] = -FONE;
  56.     else
  57.         p->dda[Y] = muldiv (MM[Y], FONE/10*VONE, I[Y][Y]);
  58.  
  59.     p->dda[Z] = muldiv (MM[Z], FONE/10*VONE, I[Z][Z]);
  60.  
  61.     tt = muldiv (p->da[Y], p->da[Z], t);
  62.     p->dda[X] -= muldiv (tt, I[Z][Z] - I[Y][Y], I[X][X]);
  63.  
  64.     tt = muldiv (p->da[X], p->da[Z], t);
  65.     p->dda[Y] -= muldiv (tt, I[Z][Z] - I[X][X], I[Y][Y]);
  66.  
  67.     tt = muldiv (p->da[Y], p->da[X], t);
  68.     p->dda[Z] -= muldiv (tt, I[Y][Y] - I[X][X], I[Z][Z]);
  69. } else {
  70.     double    u, v, w, _p, q, r;
  71.     double    L, M, N, XX, YY, ZZ;
  72.     double    Ixx, Iyy, Izz, Ixz;
  73.     double    Dp, Dq, Dr, Du, Dv, Dw;
  74.     double    VtoM, AtoR;
  75.     double    f;
  76.  
  77. /* These are the 6DOF rigid body equations. They are presented in over-
  78.  * simplified form (assume Jxz is negligible), rough form (assume Jxz is
  79.  * small so ignore Ixz^2), complete but optimized (using C0-C10) and raw
  80.  * form.
  81.  *
  82.  * It is all done in body axes.
  83.  *
  84.  * Note that the forces and moments are divided by the weight but this has
  85.  * no effect on the calcs.
  86. */
  87.  
  88. /* First convert state to SI units.
  89.  * Forces and moments are scaled by 1/M.
  90.  * Inertia moments are scaled by 1/(10*M).
  91. */
  92.     AtoR = (double)VONE*C_PI/2/FONE;    /* Angles->Radians */
  93.     VtoM = 1/(double)VONE;            /* Vunits->Meters */
  94.  
  95.     u  =  EX->v[Y] *VtoM;
  96.     v  =  EX->v[X] *VtoM;
  97.     w  = -EX->v[Z] *VtoM;
  98.  
  99.     _p =  p->da[Y] *AtoR;
  100.     q  =  p->da[X] *AtoR;
  101.     r  = -p->da[Z] *AtoR;
  102.  
  103.     L  =  MM[Y] *AtoR*VONE;
  104.     M  =  MM[X] *AtoR*VONE;
  105.     N  = -MM[Z] *AtoR*VONE;
  106.  
  107.     XX =  F[Y] *VtoM;
  108.     YY =  F[X] *VtoM;
  109.     ZZ = -F[Z] *VtoM;
  110.  
  111.     Ixx = I[Y][Y] *10.0/FONE;
  112.     Iyy = I[X][X] *10.0/FONE;
  113.     Izz = I[Z][Z] *10.0/FONE;
  114.     Ixz = I[Y][Z] *10.0/FONE;
  115.  
  116. /* Get acceleration about c.g.
  117. */
  118.     Du = XX -  q*w + r*v;
  119.     Dv = YY + _p*w - r*u;
  120.     Dw = ZZ - _p*v + q*u;
  121.  
  122. /* Get rotations about c.g.
  123. */
  124.  
  125. #if 0
  126. /* The plain 6DOF calculations. Of course, even here we assume that Jxz is
  127.  * the only significant small cross term.
  128. */
  129. {
  130.     double    Irr, Ppq, Pqr, Pn, Pl, Qpp, Qpr, Qrr, Qm, Rpq, Rqr, Rn,
  131.         Rl;
  132.  
  133.     Irr = Ixx*Izz - Ixz*Ixz;
  134.  
  135.     Ppq = (Ixx - Iyy + Izz)*Ixz/Irr;
  136.     Pqr = ((Iyy - Izz)*Izz - Ixz*Ixz)/Irr;
  137.     Pn  = Ixz/Irr;
  138.     Pl  = Izz/Irr;
  139.  
  140.     Qpp = -Ixz/Iyy;
  141.     Qpr = (Izz - Ixx)/Iyy;
  142.     Qrr = Ixz/Iyy;
  143.     Qm  = 1/Iyy;
  144.  
  145.     Rpq = ((Ixx - Iyy)*Ixx + Ixz*Ixz)/Irr;
  146.     Rqr = (Iyy - Izz - Ixx)*Ixz/Irr;
  147.     Rn  = Ixx/Irr;
  148.     Rl  = Ixz/Irr;
  149.  
  150.     Dp  = Ppq*_p*q  + Pqr* q*r + Pn*N    + Pl*L;
  151.     Dq  = Qpp*_p*_p + Qpr*_p*r + Qrr*r*r + Qm*M;
  152.     Dr  = Rpq*_p*q  + Rqr* q*r + Rl*L    + Rn*N;
  153. }
  154. #endif
  155.  
  156. #if 0
  157. /* These are the same formulaes with the terms reorganized.
  158. */
  159. {
  160.     double    C0, C1, C2, C3, C4, C5, C6, C7, C8, C9, C10;
  161.  
  162.     C0 = Ixx*Izz - Ixz*Ixz;
  163.     C1 = Izz/C0;
  164.     C2 = Ixz/C0;
  165.     C3 = C2*(Ixx - Iyy + Izz);
  166.     C10 = C2*Ixz;
  167.     C4 = C1*(Iyy - Izz) - C10;
  168.     C5 = 1/Iyy;
  169.     C6 = C5*Ixz;
  170.     C7 = C5*(Izz - Ixx);
  171.     C8 = Ixx/C0;
  172.     C9 = C8*(Ixx - Iyy) + C10;
  173.  
  174.     Dp = (Tp = L*C1) + N*C2 + (_p*C3 - r*C4)*q;
  175.     Dq = (Tq = M*C5) + (r+_p)*(r-_p)*C6 + r*_p*C7;
  176.     Dr = (Tr = N*C8) + L*C2 + (_p*C9 - r*C3)*q;
  177. }
  178. #endif
  179.  
  180. #if 1
  181. /* Here we assume that Ixz*Ixz is effectively zero. These can be simplified
  182.  * a bit more. Note that we can now defer the divisions so the 1/Ixx term was
  183.  * extracted from all the coefficients. C0, C1, C5, C6 and C8 were trivialized
  184.  * and C10 is zero.
  185. */
  186. {
  187.     double    C2, C3, C4, C6, C7, C9;
  188.     double    pq, rq;
  189.  
  190.     C2 = Ixz/Izz;
  191.     C3 = C2*(Ixx - Iyy + Izz);
  192.     C4 = Iyy - Izz;
  193.     C6 = Ixz;
  194.     C7 = Izz - Ixx;
  195.     C9 = Ixx - Iyy;
  196.  
  197.     pq = _p*q;
  198.     rq =  r*q;
  199.  
  200.     Dp = (L + N*C2 + pq*C3 - rq*C4)/Ixx;
  201.     Dq = (M + (r+_p)*(r-_p)*C6 + r*_p*C7)/Iyy;
  202.     Dr = (N + L*C2 + pq*C9 - rq*C3)/Izz;
  203. }
  204. #endif
  205.  
  206. #if 0
  207. /* Here we assume that Ixz is effectively zero.
  208.  * we get C2 = C3 = C6 = 0 which leaves us with C4, C7 and C9.
  209. */
  210.     Dp = (L +  q*r*(Izz - Iyy))/Ixx;
  211.     Dq = (M + _p*r*(Izz - Ixx))/Iyy;
  212.     Dr = (N + _p*q*(Ixx - Iyy))/Izz;
  213. #endif
  214.  
  215. #if 0
  216. /* Here we assume small rates and thus ignore all but the simple angular
  217.  * velocity that each axis incures.
  218. */
  219.     Dp = L/Ixx;
  220.     Dq = M/Iyy;
  221.     Dr = N/Izz;
  222. #endif
  223.  
  224.     EX->a[X] =  (int)(Dv /VtoM);
  225.     EX->a[Y] =  (int)(Du /VtoM);
  226.     EX->a[Z] = -(int)(Dw /VtoM);
  227.  
  228.     EX->Gforce = -(int)(ZZ/VtoM);
  229.  
  230.     p->dda[X] =  (int)(Dq /AtoR);
  231.  
  232.     f =  Dp /AtoR;
  233.     if (f > (double)FONE)
  234.         f = (double)FONE;
  235.     else if (f < -(double)FONE)
  236.         f = -(double)FONE;
  237.     p->dda[Y] = (int)f;
  238.  
  239.     p->dda[Z] = -(int)(Dr /AtoR);
  240. }
  241.     EX->Gforce += fmul (GACC, p->T[Z][Z]);
  242.  
  243. /* The if() in the next segment are for stabilizing the simulation. Since
  244.  * we use integers, and the simulation is discrete (not continuous), it is
  245.  * likely that a 'standing still' craft will actually appear to be badly
  246.  * shiverring instead. This is the result of applying the friction force
  247.  * one way, then the other way on the next frame. We avoid this by forcing
  248.  * the rates to become zero before changing direction. Well, yes, you could
  249.  * call it a kludge.
  250. */
  251.     t = EX->v[X];
  252.     EX->v[X] += TADJ(EX->a[X]);
  253.     if (onground && ((t > 0 && EX->v[X] < 0) || (t < 0 && EX->v[X] > 0)))
  254.         EX->v[X] = 0;
  255.         
  256.     t = EX->v[Y];
  257.     EX->v[Y] += TADJ(EX->a[Y]);
  258.     if (onground && ((t > 0 && EX->v[Y] < 0) || (t < 0 && EX->v[Y] > 0)))
  259.         EX->v[Y] = 0;
  260.  
  261.     t = EX->v[Z];
  262.     EX->v[Z] += TADJ(EX->a[Z]);
  263.     if (onground && ((t > 0 && EX->v[Z] < 0) || (t < 0 && EX->v[Z] > 0)))
  264.         EX->v[Z] = 0;
  265.  
  266.     t = p->da[X];
  267.     p->da[X] += TADJ(p->dda[X]);
  268.     if (onground && ((t > 0 && p->da[X] < 0) || (t < 0 && p->da[X] > 0)))
  269.         p->da[X] = 0;
  270.  
  271.     t = p->da[Y];
  272.     p->da[Y] += TADJ(p->dda[Y]);
  273.     if (onground && ((t > 0 && p->da[Y] < 0) || (t < 0 && p->da[Y] > 0)))
  274.         p->da[Y] = 0;
  275.  
  276.     t = p->da[Z];
  277.     p->da[Z] += TADJ(p->dda[Z]);
  278.     if (onground && ((t > 0 && p->da[Z] < 0) || (t < 0 && p->da[Z] > 0)))
  279.         p->da[Z] = 0;
  280.  
  281.     AVcopy (dae, p->dae);
  282.  
  283.     Euler (p);
  284. #if 0
  285.     p->a[X] += (t = p->dae[X] + (p->dae[X]-dae[X])/2,    /* Adams 2nd */
  286.         iabs(t) < VMAX/VONE ? TADJ(t*VONE) : TADJ(t)*VONE);
  287.     p->a[Y] += (t = p->dae[Y] + (p->dae[Y]-dae[Y])/2,
  288.         iabs(t) < VMAX/VONE ? TADJ(t*VONE) : TADJ(t)*VONE);
  289.     p->a[Z] += (t = p->dae[Z] + (p->dae[Z]-dae[Z])/2,
  290.         iabs(t) < VMAX/VONE ? TADJ(t*VONE) : TADJ(t)*VONE);
  291. #endif
  292. #if 0
  293.     p->a[X] += (t = (dae[X]+p->dae[X])/2,            /* trapezoid */
  294.         iabs(t) < VMAX/VONE ? TADJ(t*VONE) : TADJ(t)*VONE);
  295.     p->a[Y] += (t = (dae[Y]+p->dae[Y])/2,
  296.         iabs(t) < VMAX/VONE ? TADJ(t*VONE) : TADJ(t)*VONE);
  297.     p->a[Z] += (t = (dae[Z]+p->dae[Z])/2,
  298.         iabs(t) < VMAX/VONE ? TADJ(t*VONE) : TADJ(t)*VONE);
  299. #endif
  300. #if 1
  301.     p->a[X] += (t = p->dae[X],                /* rectangle */
  302.         iabs(t) < VMAX/VONE ? TADJ(t*VONE) : TADJ(t)*VONE);
  303.     p->a[Y] += (t = p->dae[Y],
  304.         iabs(t) < VMAX/VONE ? TADJ(t*VONE) : TADJ(t)*VONE);
  305.     p->a[Z] += (t = p->dae[Z],
  306.         iabs(t) < VMAX/VONE ? TADJ(t*VONE) : TADJ(t)*VONE);
  307. #endif
  308.  
  309.     if (iabs (p->a[X]) > D90) {
  310.         p->a[X] = D180 - p->a[X];
  311.         p->a[Y] += D180;
  312.         p->a[Z] += D180;
  313.     }
  314. }
  315. #undef FUNDAMENTAL
  316.